#include <arch/smp.h>
#include <arch/lapic.h>
#include <arch/processor.h>
#include <arch/io.h>
#include <arch/intr.h>

#include <mm/page_allocator.h>
#include <mm/paging.h>

#include <drv/hpet.h>
#include <drv/serial.h>

#include <utils/memutils.h>

typedef struct __packed {
	uint64_t cr3;
	uintptr_t stack;
	void *ap_main;
	uint32_t is_ready;
} ap_bootstrap_data_t;

static volatile ap_bootstrap_data_t *bootstrap_data = (ap_bootstrap_data_t *)0x7c00;
static uintptr_t ap_bootstrap_address = 0x8000;

incbin_extern(ap_boot);

static void ap_ipi_sender(processor_t *processor) {
	if(processor->attribute & PROCESSOR_BSP) return;

	serial_printf("sending ipi to ap %u\n", processor->apic_id);

	bootstrap_data->stack = mm_pallocator_alloc4k(4);
	bootstrap_data->is_ready = 0;

	lapic_send_ipi(processor->apic_id, IPI_INIT | IPI_ASSERT, 0);
	lapic_send_ipi(processor->apic_id, IPI_INIT, 0);

	hpet_sleep(100000);

	for(uint64_t i = 0; i < 2; ++i) {
		lapic_send_ipi(processor->apic_id, IPI_STARTUP, ap_bootstrap_address >> PAGE_4KSHIFT);
	}

	hpet_sleep(1000000);

	while(!bootstrap_data->is_ready);
}

static void ap_main() {
	uint32_t apic_id = lapic_ap_init();

	processor_self_init();
	intr_load_idt();

	serial_printf("ap %u is ready\n", apic_id);

	bootstrap_data->is_ready = 1;

	lapic_start_timer();

	while(1);
}

void smp_init() {
	__asm__ volatile("mov %%cr3, %0" : "=r"(bootstrap_data->cr3));
	bootstrap_data->ap_main = &ap_main;

	memcpy((void *)ap_bootstrap_address, &__ap_boot, incbin_sizeof(ap_boot));

	processor_foreach(ap_ipi_sender);
}